#include <Arduino.h>
#include <SoftwareSerial.h>

// Modbus从站地址
#define MODBUS_SLAVE_ADDR 1

// Modbus功能码
#define MODBUS_FC_READ_COILS 1
#define MODBUS_FC_READ_DISCRETE_INPUTS 2
#define MODBUS_FC_READ_HOLDING_REGISTERS 3
#define MODBUS_FC_READ_INPUT_REGISTERS 4
#define MODBUS_FC_WRITE_SINGLE_COIL 5
#define MODBUS_FC_WRITE_SINGLE_REGISTER 6

// Modbus串口引脚定义
#define MODBUS_RX_PIN 10
#define MODBUS_TX_PIN 11

// 寄存器定义
#define REG_INPUT_START 0
#define REG_INPUT_NREGS 4
#define REG_HOLDING_START 0
#define REG_HOLDING_NREGS 4

// 创建SoftwareSerial对象用于Modbus通信
SoftwareSerial modbusSerial(MODBUS_RX_PIN, MODBUS_TX_PIN);

// 寄存器数据
uint16_t inputRegisters[REG_INPUT_NREGS] = {0};
uint16_t holdingRegisters[REG_HOLDING_NREGS] = {0};

// Modbus通信状态
enum ModbusState {
  WAITING_ADDRESS,
  WAITING_FUNCTION,
  WAITING_DATA,
  WAITING_CRC
};

ModbusState currentState = WAITING_ADDRESS;
uint8_t currentSlaveAddr = 0;
uint8_t currentFunction = 0;
uint8_t dataBuffer[256];
uint8_t dataIndex = 0;
uint16_t crcValue = 0;

// 计算Modbus CRC
uint16_t calculateCRC(uint8_t* buffer, uint8_t length) {
  uint16_t crc = 0xFFFF;
  for (uint8_t i = 0; i < length; i++) {
    crc ^= buffer[i];
    for (uint8_t j = 0; j < 8; j++) {
      if (crc & 0x0001) {
        crc >>= 1;
        crc ^= 0xA001;
      } else {
        crc >>= 1;
      }
    }
  }
  return crc;
}

// 初始化Modbus通信
void modbusInit(long baudRate) {
  modbusSerial.begin(baudRate);
  pinMode(MODBUS_RX_PIN, INPUT);
  pinMode(MODBUS_TX_PIN, OUTPUT);
  currentState = WAITING_ADDRESS;
}

// 处理Modbus请求
void modbusProcess() {
  while (modbusSerial.available() > 0) {
    uint8_t byte = modbusSerial.read();
    
    switch (currentState) {
      case WAITING_ADDRESS:
        if (byte == MODBUS_SLAVE_ADDR) {
          currentSlaveAddr = byte;
          dataBuffer[0] = byte;
          dataIndex = 1;
          currentState = WAITING_FUNCTION;
        }
        break;
        
      case WAITING_FUNCTION:
        currentFunction = byte;
        dataBuffer[dataIndex++] = byte;
        currentState = WAITING_DATA;
        break;
        
      case WAITING_DATA:
        dataBuffer[dataIndex++] = byte;
        // 根据功能码确定数据长度，这里简化处理
        if (dataIndex >= 8) { // 最小请求长度
          currentState = WAITING_CRC;
        }
        break;
        
      case WAITING_CRC:
        // 简化的CRC处理
        if (dataIndex < 255) {
          dataBuffer[dataIndex++] = byte;
          // 检查是否接收完CRC
          if (dataIndex >= 8) { // 最小帧长度
            // 验证CRC（这里简化处理）
            // 处理请求
            modbusHandleRequest();
            currentState = WAITING_ADDRESS;
          }
        } else {
          // 数据溢出，重置状态
          currentState = WAITING_ADDRESS;
        }
        break;
    }
  }
}

// 处理Modbus请求
void modbusHandleRequest() {
  switch (currentFunction) {
    case MODBUS_FC_READ_HOLDING_REGISTERS:
      // 读取保持寄存器
      modbusReadHoldingRegisters();
      break;
      
    case MODBUS_FC_WRITE_SINGLE_REGISTER:
      // 写入单个寄存器
      modbusWriteSingleRegister();
      break;
      
    default:
      // 不支持的功能码
      modbusSendException(0x01); // 非法功能
      break;
  }
}

// 读取保持寄存器
void modbusReadHoldingRegisters() {
  // 简化实现，实际应解析请求参数
  uint8_t response[8 + REG_HOLDING_NREGS * 2];
  response[0] = MODBUS_SLAVE_ADDR;
  response[1] = MODBUS_FC_READ_HOLDING_REGISTERS;
  response[2] = REG_HOLDING_NREGS * 2;
  
  // 填充寄存器数据
  for (uint8_t i = 0; i < REG_HOLDING_NREGS; i++) {
    response[3 + i * 2] = (holdingRegisters[i] >> 8) & 0xFF;
    response[4 + i * 2] = holdingRegisters[i] & 0xFF;
  }
  
  // 计算CRC
  uint16_t crc = calculateCRC(response, 3 + REG_HOLDING_NREGS * 2);
  response[3 + REG_HOLDING_NREGS * 2] = crc & 0xFF;
  response[4 + REG_HOLDING_NREGS * 2] = (crc >> 8) & 0xFF;
  
  // 发送响应
  for (uint8_t i = 0; i < 5 + REG_HOLDING_NREGS * 2; i++) {
    modbusSerial.write(response[i]);
  }
}

// 写入单个寄存器
void modbusWriteSingleRegister() {
  // 简化实现，实际应解析请求参数
  uint16_t regAddr = ((uint16_t)dataBuffer[2] << 8) | dataBuffer[3];
  uint16_t regValue = ((uint16_t)dataBuffer[4] << 8) | dataBuffer[5];
  
  if (regAddr >= REG_HOLDING_START && regAddr < REG_HOLDING_START + REG_HOLDING_NREGS) {
    holdingRegisters[regAddr - REG_HOLDING_START] = regValue;
    
    // 发送确认响应
    uint8_t response[8];
    response[0] = MODBUS_SLAVE_ADDR;
    response[1] = MODBUS_FC_WRITE_SINGLE_REGISTER;
    response[2] = dataBuffer[2];
    response[3] = dataBuffer[3];
    response[4] = dataBuffer[4];
    response[5] = dataBuffer[5];
    
    // 计算CRC
    uint16_t crc = calculateCRC(response, 6);
    response[6] = crc & 0xFF;
    response[7] = (crc >> 8) & 0xFF;
    
    // 发送响应
    for (uint8_t i = 0; i < 8; i++) {
      modbusSerial.write(response[i]);
    }
  } else {
    // 地址超出范围
    modbusSendException(0x02);
  }
}

// 发送异常响应
void modbusSendException(uint8_t exceptionCode) {
  uint8_t response[5];
  response[0] = MODBUS_SLAVE_ADDR;
  response[1] = currentFunction | 0x80; // 设置异常标志
  response[2] = exceptionCode;
  
  // 计算CRC
  uint16_t crc = calculateCRC(response, 3);
  response[3] = crc & 0xFF;
  response[4] = (crc >> 8) & 0xFF;
  
  // 发送响应
  for (uint8_t i = 0; i < 5; i++) {
    modbusSerial.write(response[i]);
  }
}

// 更新输入寄存器值（从主程序调用）
void modbusUpdateInputRegisters(float* sensorValues) {
  for (int i = 0; i < REG_INPUT_NREGS; i++) {
    // 将浮点数转换为整数进行存储
    inputRegisters[i] = (uint16_t)(sensorValues[i] * 1000);
  }
}

// 设置保持寄存器值（从主程序调用）
void modbusSetHoldingRegister(uint8_t index, uint16_t value) {
  if (index < REG_HOLDING_NREGS) {
    holdingRegisters[index] = value;
  }
}

// 获取保持寄存器值（从主程序调用）
uint16_t modbusGetHoldingRegister(uint8_t index) {
  if (index < REG_HOLDING_NREGS) {
    return holdingRegisters[index];
  }
  return 0;
}